#!/usr/bin/env python
# -*- coding: utf-8 -*-  
import rospy
from sensor_msgs.msg import Image
from uav.msg import position
import cv2
from cv_bridge import CvBridge
import numpy as np


# 圆心识别程序
def getContours(img,imgContour):
    pc=0
    img, contours,hierarchy =cv2.findContours(img,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_NONE)
    #contours =cv2.findContours(img,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_NONE)
    for cnt in contours:
        area = cv2.contourArea(cnt)            
        if area>600:             
            peri = cv2.arcLength(cnt,True)
            approx = cv2.approxPolyDP(cnt,0.02*peri,True)                          
            x, y, w, h =cv2.boundingRect(approx)
            aspRatio = w/float(h)
            if aspRatio>0.95 and aspRatio<1.05 :
               cv2.circle(imgContour,(int(x+w/2),int(y+h/2)),10,(0,0,255),3)
               pc=[int(x+w/2), int(y+h/2)]
    return pc


# 读取摄像头图像
cap = cv2.VideoCapture(0)
cap.set(3,640)
cap.set(4,480)
cap.set(10,100)


# 主程序
def target_publish():

    rospy.init_node('target_pub', anonymous=True)
    target_pub = rospy.Publisher('/uav/target', position, queue_size=1)
    rate = rospy.Rate(20) 

    # 初始化目标坐标值
    target = position()
    target.x = 0.0
    target.y = 0.0
    target.z = 1.0

    # 主循环，始终发布识别结果
    while not rospy.is_shutdown():
        # 图像前处理
        success,img =cap.read()
        imgContour =img.copy()

        imgGray =cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
        imgBlur = cv2.GaussianBlur(imgGray,(5,5),1)

        imgCanny =cv2.Canny(imgBlur,80,80)
        pc=getContours(imgCanny,imgContour)

        resimg = cv2.resize(imgContour, (240, 240))

        cv2.imshow("contour",resimg)
        cv2.waitKey(20)

        # 识别成功则转换坐标系发布，否则发布0值
        if pc != 0:
            target.x = (pc[0]-320.0)/640.0
            target.y = (pc[1]-240.0)/480.0
        else:
			target.x = 0.0
			target.y = 0.0 
            
        target_pub.publish(target)
        rate.sleep()

if __name__ == "__main__":
    target_publish()

